/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef MOVEFREEINSTRUCTIONLOGIC_H
#define MOVEFREEINSTRUCTIONLOGIC_H

#include "instructionEnum.h"
#include "GeneralDefine.h"
#include "moveFreeInstruction.h"
#include "fleetdefine.h"
#include <QMap>

#define D_SPLINE_CONTROL_RATIO 0.38;

class ProgramSet;
class McControllerThread;
class ProgramEngine;
/**
 * @brief The MoveFreeInstructionLogic class  指令逻辑处理类
 */
class MoveFreeInstructionLogic
{
public:
    MoveFreeInstructionLogic(int robotIdIn, ProgramSet* programSetIn,ProgramEngine* programEngineIn,
                             McControllerThread *programMcControllerIn,QMap <int, DPoint> *vehicleDPointIn);

public:
    /**
     * @brief startMoveFree.  ProgramTextTransfer类需要有提取ＤＰoint数据的函数并传递进来。
     * @param programNumIn
     * @param instructionIn
     * @return
     */
    E_PROGRAM_LOOP_RESULT startMoveFree(int programNumIn, const MoveFreeInstruction &instructionIn, int lineIndex);
    E_PROGRAM_LOOP_RESULT moveToPointByPathStyle_blockWait(const MoveFreeInstruction &instructionIn, int lineIndex,
                                      bool isDirectPoint, DPoint targetPoint, int programNumIn=0);
    //自动躲避障碍物,自动规划路径
    E_PROGRAM_LOOP_RESULT moveToPointByAutoBarrier_blockWait(const MoveFreeInstruction &instructionIn, int lineIndex,
                                      bool isDirectPoint, DPoint targetPoint, int programNumIn=0);

    //使用路径地图的最短路径,到达目标站点
    E_PROGRAM_LOOP_RESULT moveToPointByBestPathMap_blockWait(const MoveFreeInstruction &instructionIn, int lineIndex,
                                       int programNumIn=0);
    /**
     * @brief moveToPoint_noWait 函数执行完成后，运动配置参数不会还原。
     * @param instructionIn
     * @param lineIndex
     * @param isDirectPoint
     * @param targetPoint
     * @param programNumIn
     * @return
     */
    E_PROGRAM_LOOP_RESULT moveToPointByPathStyle_noWait(const MoveFreeInstruction &instructionIn, int lineIndex,
                                      bool isDirectPoint, DPoint targetPoint, int programNumIn=0);
    E_PROGRAM_LOOP_RESULT moveToMultiPointByPathStyle_run(const QVector<int> &lineList,
                                   const QVector<LaserPathParameter> &laserPathListParaIn, int isLastPointMove=0);
    //only hmi调试用
    int moveToNavgationPoint(MovingToNavagationParameter parameterIn,int &returnFlag);

    int isMotionInstructionFinished(int waitLine);
    int isTargetPointArrived(VehiclePosition targetPoint, double allowBias);
    int processMotionInstruction_multiMove(int programNum, int startLine);
    int processMotionInstruction_oneMove(const MoveFreeInstruction &instructionIn, int startLine,bool isDirectPoint,DPoint targetPoint);
    int reset();
    int getSetMotionBlock(int startLineNum);
    /**
     * @brief pretreatMotion    预处理运动指令　hualei 实际上没起作用
     * @param programNum        程序标号
     * @return
     */
    int pretreatMotion();


private:
    int getCommandFromQueue(QQueue<FleetCommand> &fleetCommandQueue,FleetCommand &moveParameter);
    int processAutoDrivePathQueue(QQueue<FleetCommand> &fleetCommandQueue);
    bool isNeedAddTraj(int programNum, int startLine);
    E_PROGRAM_LOOP_RESULT moveSetting(const MoveFreeInstruction &instructionIn);
    E_PROGRAM_LOOP_RESULT moveSetting_pathRoute(const MoveFreeInstruction &instructionIn);
    //一直加轨迹。暂停就不能返回，防止下次逻辑处理问题.暂时不支持自动壁障导航
    E_PROGRAM_LOOP_RESULT addProgramMoveCommand_fleet(const QVector<int> &lineList,
                                                      const QVector<LaserPathParameter> &laserPathListParaIn);
    //判断如果添加过轨迹，则不在继续加轨迹。
    E_PROGRAM_LOOP_RESULT addProgramMoveCommand(const MoveFreeInstruction &instructionIn,int lineIndex,
                                         bool isDirectPoint, DPoint targetPoint,int programNumIn);
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish_fleet(const QVector<int> &lineList);
    //暂停必须在里面等待。quickstop.停止退出
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish_fleet(int lineIndex);
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish(const MoveFreeInstruction &instructionIn, int lineIndex,
                                               bool isDirectPoint, DPoint targetPoint, int programNumIn);
    E_PROGRAM_LOOP_RESULT waitMovebaseTargetAccomplish(VehiclePosition targetPoint, double allowBias);
    int readConfigFile(QString fileName);
    int initialTestPoint();
    int quitClear();
    
    

    void addMsg( int messageLevel,
                 std::string componentName,
                 std::string messageType,
                 int messageCode,
                 int robotId,
                 int parameter1 = 0,
                 int parameter2 = 0,
                 int parameter3 = 0,
                 int parameter4 = 0,
                 QString message= "");

private:
    int robotId;
    ProgramSet* programSet;
    /**
     * @brief programMcController       算法线程指针
     */
    McControllerThread *programMcController;
    ProgramEngine* programEngine;

    QMap <int, DPoint> *vehicleDPoint;

    int lastMotionInstructionLine;

//    bool waitFlag;
    double vmark_lineAcceleration;
    double vmark_rotateAcceleration;
    double reflect_lineAcceleration;
    double reflect_rotateAcceleration;
    double agv_lineMaxAcceleration;
    double agv_rotateMaxAcceleration;

    double cu1PositionAllowAngleAccuracy;//degree

    QMap<int,LaserPathParameter> pathSettingInfo;//路径的壁障信息缓存
};

#endif // MOVEFREEINSTRUCTIONLOGIC_H
